#include "chd_turtle.h"

CHD_Turtle::CHD_Turtle(ros::NodeHandle* nh)
{
    nh_ = *nh;
    vel_pub = nh_.advertise<geometry_msgs::Twist>("/turtle1/cmd_vel",10);
    pose_sub = nh_.subscribe<turtlesim::Pose>("/turtle1/pose", 10, &CHD_Turtle::pose_callback, this);
}

void CHD_Turtle::pose_callback(const turtlesim::PoseConstPtr& msg)
{
    // 将接收到的消息打印出来
    ROS_INFO("Turtle pose: x:%0.6f, y:%0.6f", msg->x, msg->y);

    geometry_msgs::Twist vel_msg;
    vel_msg.linear.x = 1.0;

    // 撞墙转弯
    if( fabs(msg->x) > 10 || fabs(msg->y) > 10)
        vel_msg.angular.z = 1.0;

    vel_pub.publish(vel_msg);
}